import pysoem
import time
import numpy as np

master = pysoem.Master()
master.open('eth1')
master.config_dc()

if master.config_init() > 0:
    master.config_map()
    
    for slave in master.slaves:
        print(f"slave name is {slave.name}")
        print(f"slave state is {slave.state}")
        print(f"slave id is {slave.id}")

    master.state_check(pysoem.SAFEOP_STATE, 50000)
    master.state_check(pysoem.OP_STATE, 50000)
    master.write_state()
    master.in_op = True
    try:
        while True:
            master.send_processdata()
            master.receive_processdata(2000)
            slave = master.slaves[0]
            # if master.state != pysoem.OP_STATE:
            #     print("Not in OP state, error code = %d" % master.state)
            #     break

            # 读取 TxPDO 数据
            slave = master.slaves[0]
            
            
            # 假设 NET_1 到 NET_16 每个数据占用 4 字节 (32 位)
            data_offset = 0
            data_length = 4
            
            force_list = np.zeros(16, dtype=int)
            for i in range(1, 17):
                data_end = data_offset + data_length
                value = int.from_bytes(slave.input[data_offset:data_end], byteorder='little', signed=True)
                force_list[i-1] = value
                # print(f"NET_{i}: {value}")
                data_offset += data_length
            
            print(f"force_list from test.py: {force_list}")
            
            # 添加一个小延迟以避免 CPU 占用过高
            time.sleep(0.0007)

    except KeyboardInterrupt:
        print("KeyboardInterrupt")
    # print(slave.input)

    finally:
        master.state = pysoem.INIT_STATE
        master.write_state()
        master.close()